/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the WaterPlus nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/*!******************************************************************
 @author     ZhangWanjie
 ********************************************************************/
#include "SDUMission.h"


CSDUMission::CSDUMission()
{
}

CSDUMission::~CSDUMission()
{
}

/**************************************************
 * 任务系统初始化
 * ************************************************/

// 预置任务：全员离开充电桩
void CSDUMission::Preset_LeaveDock()
{
    stMission new_mis;
    stTask nt;
    // [1] 1号离开充电坞，2号卡在这个任务里先别动
    RobotLeaveDock(1 , 2 , 0 , 0);
    // [2] 1号走去抓取航点，2号离开充电桩
    RobotPathToTask(1,"2");
    // 2号离开后，直接去4航点待命
    RobotLeaveDock(2, 0 , 0, 0);
    RobotPathToTask(2,"4");

}

static int arBackToDock[2];
// 预置任务：全员去充电 
void CSDUMission::Preset_GoCharging()
{
    RobotGoCharging(1);
    RobotGoCharging(2);

    // 停止
    bPause = false;
}

void CSDUMission::Start()
{
    Preset_LeaveDock();   //让机器人离开充电坞
    bPause = false;
}

/**************************************************
 * 任务切换
 * ************************************************/
void CSDUMission::MissionComplete(stMission* inMission)
{
    ROS_WARN("[CSDUMission] Mission任务 \"%s\" 执行完毕!! ",inMission->mission.c_str());

    /***********************************************
    1号到达抓取航点1后，开始新一轮循环
    ***********************************************/
    int nFind = inMission->mission.find("1号循线去往航点2");
    if(nFind >= 0)
    {
        // 1号在航点2抓取，然后去航点1，放置
        GrabStorageRack(1);
        RobotPathToTask(1,"1");
        PlaceStorageRack(1);
        // 1号到航点3待机，2号等1号到位（同步点！）
        WaitPathToTask(1,"3",2);
        //  2号去航点1抓取，然后到航点2，放置
        RobotPathToTask(2,"1");
        GrabStorageRack(2);
        RobotPathToTask(2,"2");
        PlaceStorageRack(2);
        // [4] 1号到航点3待机，2号等1号到位（同步点！）
        WaitPathToTask(2,"4",1);
        // 新一轮激活点
        RobotPathToTask(1,"2");
    }
}

// 信号1
void CSDUMission::Signal_1()
{
    // RobotPathToTask(1,"1");
    // RobotPathToTask(2,"2");
    // RobotPathToTask(3,"3");
    // RobotPathToTask(4,"4");
}

// 信号2
void CSDUMission::Signal_2()
{
    // GrabStorageRack(1);
    // RobotGotoTask(1,"5");
    // PlaceMobileRack(1,3);

    // GrabStorageRack(2);
    // RobotGotoTask(2,"6");
    // PlaceMobileRack(2,4);
}

// 信号3
void CSDUMission::Signal_3()
{
    // RobotsSyncPathTo(3,"4",  4, "3");
    // RobotsSyncPathTo(3,"3",  4, "4");
}